Option explicit
Option default none
'
' Versa Guage Altimeter (R6150V driver)
'
Const i2caddr=&b1110111 ' BMP1800 IC address
Const MS7=7 'set default wait period
Const signed=1
Const unsigned=0
Const buffersize = 2500
Const triangles=128
Const pwm_pin=11 'PWM pin for Display Backlight drive

Dim i2cin$ length 8 'max size for integer conversion
Dim UT%,UP%
Dim As integer ac1%,ac2%,ac3,ac4%,ac5%,ac6%,b1%,b2%,mb%,mc%,md% 'bmp180 parameters
Dim As integer x1%,x2%,b5%,b6%,x3,b3%,b4%,b7%,OSS%
Dim temperature%,pressure%
Dim As float altitude,ALT_old,ALT_avrage,ALT_feet,QNH,pressureinHpa
Dim OSSdata%(4)
Dim OSSscale%(4)

OSS%=1 'set oversampling ratio
OSSdata%(0)=&H34 'commands to sample pressure% with different levels of oversampling
OSSdata%(1)=&H74
OSSdata%(2)=&Hb4
OSSdata%(3)=&HF4
OSSscale%(0)=1 'scale factors for calcs when oversampled
OSSscale%(1)=2
OSSscale%(2)=4
OSSscale%(3)=8

Dim integer i,first=1,offset,buff(buffersize)
Dim integer nt=4 'Number of triangles being updated
Dim integer xx0(nt*2-1),yy0(nt*2-1),xx1(nt*2-1),yy1(nt*2-1),xx2(nt*2-1),yy2(nt*2-1),tcol(nt*2-1)
Dim integer scale1_divisor=1,select_button_touched,setup_mode,ALT_value,needle_value,setup_screen
Dim integer ALT_changed, needle_changed, QNH_changed

Dim float QNH_in_of_mg

SetPin 23, intl, select_button_int ' The encoder button pin
SetPin 30, intl, A_int ' Encoder pin A
SetPin 31, din ' Encoder pin B (data)

I2C OPEN 400,1000 ' IIC Port 400khz, 1sec timeout
SetPin pwm_pin,dout ' Display Backlight
Pin(pwm_pin)=1 ' Just turn ON (for now)

QNH=1000 ' default sea level presure ' if nothing ever stored
VAR restore ' last user saved QNH and needles value

QNH_in_of_mg=QNH*0.0295301 ' Hpa/mb to in/mg
QNH_in_of_mg=QNH_in_of_mg*100
QNH_in_of_mg=Cint(QNH_in_of_mg)
QNH_in_of_mg=QNH_in_of_mg/100

CLS ' Clear Display Screen
alt_gauge_face ' Draw ALT Gauge background

I2C WRITE i2caddr,1,1,&HAA 'send read calibration data command
I2C READ i2caddr,0,22,i2cin$() 'read in calibration data
ac1%=intconv(Mid$(i2cin$,1,2),signed)
ac2%=intconv(Mid$(i2cin$,3,2),signed)
ac3=intconv(Mid$(i2cin$,5,2),signed)
ac4%=intconv(Mid$(i2cin$,7,2),unsigned)
ac5%=intconv(Mid$(i2cin$,9,2),unsigned)
ac6%=intconv(Mid$(i2cin$,11,2),unsigned)
b1%=intconv(Mid$(i2cin$,13,2),signed)
b2%=intconv(Mid$(i2cin$,15,2),signed)
mb%=intconv(Mid$(i2cin$,17,2),signed)
mc%=intconv(Mid$(i2cin$,19,2),signed)
md%=intconv(Right$(i2cin$,2),signed)

Do
  update_ALT
  If first Then
    ALT_avrage=altitude
  Else
    ALT_avrage=ALT_avrage*0.9 ' keep running avrage
    Alt_avrage=ALT_avrage+altitude/10
  EndIf
  ALT_feet=Cint(ALT_avrage)
  If ALT_old<>ALT_feet Then
    update_display(ALT_feet,109,110,110)
    ALT_old=ALT_feet
  EndIf
  Print "ALT avrage ="; ALT_avrage
  Print "ALT whole feet ="; ALT_feet
  Pause 1000
  check_setup
Loop

End

' -------- Subs

Sub update_ALT
  I2C WRITE i2caddr,0,2,&HF4,&H2E 'send temp conversion
  Pause MS7 'wait for temperature% conversion
  I2C WRITE i2caddr,1,1,&HF6 'send read data
  I2C READ i2caddr,0,2,i2cin$() 'read 2 bytes
  UT%=intconv(i2cin$,unsigned)
'  UT%=27898 ' Uncomment this line to check algorithm against datasheet
  I2C WRITE i2caddr,0,2,&HF4,ossdata%(oss%) 'send pressure% conversion
  Pause (oss%+1)*ms7 'wait for the p  ressure% conversion
  I2C WRITE i2caddr,1,1,&HF6 'send read data
  I2C READ i2caddr,0,3,i2cin$() 'read 3 bytes
  UP%=intconv(i2cin$,unsigned)
  UP%=UP%>>(8-oss%) 'scale the oUT%pUT% by the numb%er of unused bits in the xlsb byte
'  UP%=23843' Uncomment this line to check algorithm against datasheet
  calc_temp
  calc_pressure
  pressureinHpa=pressure%/100
  Print "Temp =";Str$(temperature%/10*9/5+32,4,1);" Deg F"
  Print "Local pressure =";Str$(pressure%/100,4,1);" Hpa/mb"
  altitude=calcaltitude(QNH,pressureinHpa)
  Print "altitude in feet =";altitude
  Print "Sea level pressure =";calcQNH(altitude,pressureinHpa);" Hpa/mb"
  QNH_in_of_mg=QNH * .0295301 ' Hpa/mb to in/mg
  Print "Sea level pressure =";QNH_in_of_mg;" in/mg"
End Sub

' calc_temperature%: calculate the temperature% from the raw temperature% given the calibration parameters
'
Sub calc_temp
  x1%=(UT%-ac6%)*ac5%\powerof2(15)
  x2%=mc%*powerof2(11)/(x1%+md%) 'This needs to be a floating divide to match the datasheet
  b5%=x1%+x2%
  temperature%=(b5%+8)\powerof2(4)
End Sub
'
' calc_pressure: calculate the pressure% from the raw pressure% given the calibration parameters and temperature% oUT%pUT%
'
Sub calc_pressure
   b6%=b5%-4000
   x1%=(b2%*(b6%*b6%/powerof2(12)))\powerof2(11)
   x2%=ac2%*b6%\powerof2(11)
   x3=x1%+x2%
   b3%=(((ac1%*4+x3)*ossscale%(oss%))+2)\4
   x1%=AC3*b6%\POWEROF2(13)
   x2%=(b1%*(b6%*b6%/POWEROF2(12)))\POWEROF2(16)
   x3=((x1%+x2%)+2)\4
   b4%=ac4%*(Abs(x3+32768))\powerof2(15)
   b7%=Abs(UP%-b3%)*(50000\ossscale%(oss%))
   pressure%=(b7%*2)\b4%
   x1%=(pressure%\powerof2(8))*(pressure%\powerof2(8))
   x1%=(x1%*3038)\powerof2(16)
   x2%=(-7357*pressure%)\powerof2(16)
   pressure%=pressure%+(x1%+x2%+3791)\powerof2(4)
   End Sub
'
Function calcQNH(currentaltitude As float,localpressure As float) As float
   calcQNH=(localpressure*100)/((1-(currentaltitude*0.3048/44330))^5.255)/100
End Function
'
Function log10(x As float) As float
  log10=Log(x)/2.302585093
End Function
'
Function calcaltitude(sealevelpressure As float, localpressure As float) As float
  Local a As float, b As float
   calcaltitude=((10^(log10(localpressure/sealevelpressure)/5.2558797))-1) /-6.8755856*1000000
End Function

Sub update_display(value As integer, size As integer, x As integer, y As integer)
    Local integer x1,y1,x2,y2,x0,y0,i
    Local float angle=value*0.36
    rotatetriangle(2,RGB(RED),angle,x,y,-2,20,2,20,-2,-size) 'make up the second hand with two triangles
    rotatetriangle(3,RGB(RED),angle,x,y,2,-size,2,20,-2,-size)
    angle=value*0.036
    rotatetriangle(0,RGB(green),angle,x,y,-size/15,0,size/15,0,0,-size*0.85)
    angle=value*0.0036
    rotatetriangle(1,RGB(yellow),angle,x,y,-size/12,0,size/12,0,0,-size*0.5)
    If first Then
      i=R61505V_V44(triangles Or nt, buff() , tcol(nt), xx0(nt), yy0(nt), xx1(nt), yy1(nt), xx2(nt), yy2(nt))
      Print "Read buffer is ",i/buffersize*100,"% used"
      first=0
    Else
      i=R61505V_V44(triangles Or (nt*2), buff() , tcol(), xx0(), yy0(), xx1(), yy1(), xx2(), yy2())
    EndIf
    Circle x,y, size\12, 0, , RGB(red), RGB(red))
    Circle x,y, size\15, 0, , 0, 0
    Circle x,y, size\20, 0, , RGB(gray), RGB(gray)
End Sub

Sub rotatetriangle(n As integer, col As integer, angle As float, x As integer, y As integer, x0 As integer, y0 As integer, x1 As integer, y1 As integer, x2 As integer, y2 As integer)
   Local float sine=Sin(Rad(angle)),cosine=Cos(Rad(angle))
   Local integer x0a,y0a,x1a,y1a,x2a,y2a
   x0a= x0*cosine - y0 * sine + x
   y0a= y0*cosine + x0 * sine + y
   x1a= x1*cosine - y1 * sine + x
   y1a= y1*cosine + x1 * sine + y
   x2a= x2*cosine - y2 * sine + x
   y2a= y2*cosine + x2 * sine + y
   xx0(n)=xx0(n+nt)
   yy0(n)=yy0(n+nt)
   xx1(n)=xx1(n+nt)
   yy1(n)=yy1(n+nt)
   xx2(n)=xx2(n+nt)
   yy2(n)=yy2(n+nt)
   xx0(n+nt)=x0a
   yy0(n+nt)=y0a
   xx1(n+nt)=x1a
   yy1(n+nt)=y1a
   xx2(n+nt)=x2a
   yy2(n+nt)=y2a
   tcol(n)=-1
   tcol(n+nt)=col
End Sub

Sub ALT_gauge_face
    CLS
    first=1 ' not sure this is correct
    scale1_divisor=1
    Local float c1,c2,tick_angle
    For c1 = 1 To 10
      draw_tic_mark(110,110,97,110,tick_angle,RGB(white))
      tick_angle=tick_angle+7.2
     For c2 = 1 To 4
       draw_tic_mark(110,110,105,110,tick_angle,RGB(white))
       tick_angle=tick_angle+7.2
     Next c2
    Next c1
    Text 112,35,"0",cm,5,1,RGB(white),RGB(black)
    Text 157,50,"1",cm,5,1,RGB(white),RGB(black)
    Text 187,85,"2",cm,5,1,RGB(white),RGB(black)
    Text 187,140,"3",cm,5,1,RGB(white),RGB(black)
    Text 152,175,"4",cm,5,1,RGB(white),RGB(black)
    Text 112,190,"5",cm,5,1,RGB(white),RGB(black)
    Text 67,175,"6",cm,5,1,RGB(white),RGB(black)
    Text 35,140,"7",cm,5,1,RGB(white),RGB(black)
    Text 35,85,"8",cm,5,1,RGB(white),RGB(black)
    Text 65,50,"9",cm,5,1,RGB(white),RGB(black)
'    Text 112,80,"ALT",cm,2,1,RGB(white),RGB(black)
    Text 60,110,"ALT",cm,2,1,RGB(white),RGB(black)
    Text 160,110,"MSL",cm,2,1,RGB(white),RGB(black)
'    Text 173,110,Str$(QNH_in_of_mg,0,2),cm,2,1,RGB(black),RGB(white)
    Text 112,140,Str$(QNH_in_of_mg,0,2),cm,2,1,RGB(black),RGB(white)
    Text 112,154,"QNH in/mg",cm,1,1,RGB(white),RGB(black)
    Circle 110,110,9,2,1,RGB(white)' create needle bezel
End Sub

Sub draw_tic_mark(x As integer, y As integer, size As integer,size2 As integer, angle As integer, col As integer)
Local integer x1,y1,x2,y2,x3,y3,dummy,mul=1,buffer(200),xx0(2),yy0(2),xx1(2),yy1(2),xx2(2),yy2(2),tcol(2)
    If scale1_divisor=0 Then mul=2
    x1=Sin(Rad(angle))*size + x ' draw first triangle
    y1=-Cos(Rad(angle))*size + y
    x2=Sin(Rad(angle))*size2 + x
    y2=-Cos(Rad(angle))*size2 + y
    x3=Sin(Rad(angle+mul))*size2 + x
    y3=-Cos(Rad(angle+mul))*size2 + y
    dummy=R61505V_V44(129, buffer(), col, x1, y1, x2, y2, x3, y3)
    x1=Sin(Rad(angle))*size + x ' draw second triangle
    y1=-Cos(Rad(angle))*size + y
    x2=Sin(Rad(angle+mul))*size2 + x
    y2=-Cos(Rad(angle+mul))*size2 + y
    x3=Sin(Rad(angle+mul))*size + x
    y3=-Cos(Rad(angle+mul))*size + y
    dummy=R61505V_V44(129, buffer(), col, x1, y1, x2, y2, x3, y3)
End Sub

Sub check_setup
    If select_button_touched Then
      setup_mode=1
      setup_screen=1 ' QNH value change
      QNH_in_of_mg=QNH*0.0295301 ' Hpa/mb to in/mg
      QNH_in_of_mg=QNH_in_of_mg*100
      QNH_in_of_mg=Cint(QNH_in_of_mg)
      QNH_in_of_mg=QNH_in_of_mg/100
      CLS RGB(138,76,0) ' Brown looking background
      Text 110,30,"Setup",cm,2,1,RGB(yellow),RGB(138,76,0) ' top line msg
      Text 110,60,"Adjust QNH",cm,2,1,RGB(yellow),RGB(138,76,0) ' next line msg
      Text 110,110,Str$(QNH_in_of_mg,0,2),cm,2,2,RGB(yellow),RGB(138,76,0))
      select_button_touched=0 ' clear flag
      Do: Loop Until select_button_touched
      select_button_touched=0 ' clear flag
      If QNH_changed Then
        QNH=QNH_in_of_mg*33.8637526
        VAR save QNH
        setup_mode=0 ' clear flags
        QNH_changed=0
        ALT_gauge_face
        Exit Sub ' all done
      EndIf
      setup_screen=2 ' ALT value change
      CLS RGB(138,76,0) ' Brown looking background
      ALT_value=ALT_feet ' copy current
      ALT_value=ALT_value/10 ' round to "10's" of feet
      ALT_value=Cint(ALT_value)
      ALT_value=ALT_value*10
      Text 110,30,"Setup",cm,2,1,RGB(yellow),RGB(138,76,0) ' top line msg
      Text 110,60,"Adjust ALT",cm,2,1,RGB(yellow),RGB(138,76,0) ' next line msg
      Text 110,110,Str$(ALT_value),cm,2,2,RGB(yellow),RGB(138,76,0))
      select_button_touched=0 ' clear flag
      Do: Loop Until select_button_touched
      select_button_touched=0 ' clear flag
      If ALT_changed Then
        QNH=calcQNH(ALT_value,pressureinHpa)
        VAR save QNH
      EndIf
      setup_mode=0 ' clear flags
      ALT_changed=0
      ALT_gauge_face
    EndIf
End Sub

Sub select_button_int ' for encoder push button
      select_button_touched=1 ' set touched flag
End Sub

Sub A_int ' Interrupt to decode the encoder output (pin A)
If setup_mode Then
   Select Case setup_screen ' Knob action depends what setup we are doing..!
    Case 1 ' QNH value change
      QNH_changed=1 ' Set flag
       If Pin(31) = 1 Then ' knob rotation less
         QNH_in_of_mg=QNH_in_of_mg-0.01
       Else
         QNH_in_of_mg=QNH_in_of_mg+0.01
       EndIf
      Box 20,90,180,40,3,RGB(yellow),RGB(138,76,0)
      Text 110,110,Str$(QNH_in_of_mg,0,2),cm,2,2,RGB(white),RGB(138,76,0))
    Case 2 ' ALT value change
      ALT_changed=1
       If Pin(31) = 1 Then
        If ALT_value>0 Then ALT_value=ALT_value-10
       Else
        ALT_value=ALT_value+10
       EndIf
      Box 20,90,180,40,3,RGB(yellow),RGB(138,76,0)
      Text 110,110,Str$(ALT_value),cm,2,2,RGB(white),RGB(138,76,0))
   End Select
EndIf
End Sub

CFunction intconv
     00000000
     27bdfff8 00001021 00001821 afa20000 afa30004 90880000 1900000b 01003821
     2503ffff 03a31821 24020001 00823021 90c60000 a0660000 24420001 00e2302a
     10c0fffa 2463ffff 8ca20000 1040000d 03a81021 9042ffff 30420080 10400009
     29020008 10400007 03a81021 27a40008 2403ffff a0430000 24420001 5444fffe
     a0430000 8fa20000 8fa30004 03e00008 27bd0008
End CFunction
'
CFunction powerof2
     00000000
     27bdfff8 00001021 00001821 afa20000 afa30004 8c820000 000218c2 03a32021
     000318c0 00431023 24030001 00431004 a0820000 8fa20000 8fa30004 03e00008
     27bd0008
End CFunction                                 